#include "multi_cavity_matrix_builder.h"
#include <iostream>
#include <fstream>
#include <sstream>
#include <cmath>
#include <algorithm>
#include <cassert>
#include <iomanip>
#include <cstring>
#include <chrono>

namespace MultiCavityMatrix {

using TensorMatMultiCavity::generate_sem_operators_1d;
using TensorMatMultiCavity::generate_chebyshev_operators;

// ------------------------------------------------------------------------
// Grid3D Implementation
// ------------------------------------------------------------------------

void Grid3D::print_info() const {
    std::cout << "Grid3D: " << imt << "x" << jmt << "x" << kmt 
              << " (" << total_points() << " total points)" << std::endl;
    std::cout << "  X range: [" << x_min << ", " << x_max << "]" << std::endl;
    std::cout << "  Y range: [" << y_min << ", " << y_max << "]" << std::endl;
    std::cout << "  Z range: [" << z_min << ", " << z_max << "]" << std::endl;
}

// ------------------------------------------------------------------------
// CavityInterface Implementation
// ------------------------------------------------------------------------

void CavityInterface::print_info() const {
    std::cout << "Interface: Cavity " << src_cavity_id << " -> Cavity " << dst_cavity_id << std::endl;
    std::cout << "  Source region: [" 
              << src_i_start << ":" << src_i_end << ", "
              << src_j_start << ":" << src_j_end << ", "
              << src_k_start << ":" << src_k_end << "]" << std::endl;
    std::cout << "  Destination start: (" 
              << dst_i_start << ", " << dst_j_start << ", " << dst_k_start << ")" << std::endl;
}

// ------------------------------------------------------------------------
// Utility Functions
// ------------------------------------------------------------------------

std::string method_to_string(DiscretizationMethod method) {
    switch (method) {
        case DiscretizationMethod::FDM:     return "Finite Difference Method";
        case DiscretizationMethod::SEM:     return "Spectral Element Method";
        case DiscretizationMethod::SPECTRAL: return "Spectral Method";
        default:                            return "Unknown Method";
    }
}

// ------------------------------------------------------------------------
// MultiCavityMatrixBuilder Implementation - Core
// ------------------------------------------------------------------------

MultiCavityMatrixBuilder::MultiCavityMatrixBuilder(DiscretizationMethod discretization_method)
    : total_grid_points(0), method(discretization_method) {
    
    // Set number of diagonals based on discretization method
    switch (method) {
        case DiscretizationMethod::FDM:
            num_diagonals = 19;  // 19-point stencil for FDM
            break;
        case DiscretizationMethod::SEM:
        case DiscretizationMethod::SPECTRAL:
            num_diagonals = 64;  // 64-point stencil for spectral methods
            break;
        default:
            num_diagonals = 19;  // Default to FDM
            DEBUG_WARN("Unknown discretization method, defaulting to FDM (19 diagonals)");
    }
    
    DEBUG_INFO("Created MultiCavityMatrixBuilder with " << method_to_string(method) 
              << " (" << num_diagonals << " diagonals)");
}

MultiCavityMatrixBuilder::~MultiCavityMatrixBuilder() {
    DEBUG_INFO("Destroying MultiCavityMatrixBuilder with " 
              << cavity_grids.size() << " cavities and "
              << interfaces.size() << " interfaces");
}

int MultiCavityMatrixBuilder::add_cavity(
    int imt, int jmt, int kmt, 
    double x_min, double x_max,
    double y_min, double y_max,
    double z_min, double z_max
) {
    // Check validity of dimensions
    if (imt <= 0 || jmt <= 0 || kmt <= 0) {
        DEBUG_ERROR("Invalid cavity dimensions: " << imt << "x" << jmt << "x" << kmt);
        return -1;
    }
    
    // Add the new cavity
    int cavity_id = cavity_grids.size();
    cavity_grids.emplace_back(imt, jmt, kmt, x_min, x_max, y_min, y_max, z_min, z_max);
    
    // Update cavity offsets and total grid points
    cavity_offsets.push_back(total_grid_points);
    total_grid_points += imt * jmt * kmt;
    
    DEBUG_INFO("Added cavity " << cavity_id << ": " 
              << imt << "x" << jmt << "x" << kmt 
              << " (" << imt * jmt * kmt << " points)");
    
    return cavity_id;
}

void MultiCavityMatrixBuilder::add_interface(
    int src_cavity_id, int dst_cavity_id,
    int src_i_start, int src_i_end, 
    int src_j_start, int src_j_end, 
    int src_k_start, int src_k_end,
    int dst_i_start, int dst_j_start, int dst_k_start
) {
    // Check if cavity IDs are valid
    if (src_cavity_id < 0 || src_cavity_id >= static_cast<int>(cavity_grids.size()) ||
        dst_cavity_id < 0 || dst_cavity_id >= static_cast<int>(cavity_grids.size())) {
        DEBUG_ERROR("Invalid cavity ID for interface: " << src_cavity_id << " -> " << dst_cavity_id);
        return;
    }
    
    // Validate source region
    const Grid3D& src_grid = cavity_grids[src_cavity_id];
    if (src_i_start < 0 || src_i_end >= src_grid.imt ||
        src_j_start < 0 || src_j_end >= src_grid.jmt ||
        src_k_start < 0 || src_k_end >= src_grid.kmt) {
        DEBUG_ERROR("Source region out of bounds in add_interface");
        return;
    }
    
    // Validate destination start
    const Grid3D& dst_grid = cavity_grids[dst_cavity_id];
    int region_width = src_i_end - src_i_start + 1;
    int region_height = src_j_end - src_j_start + 1;
    int region_depth = src_k_end - src_k_start + 1;
    
    if (dst_i_start < 0 || dst_i_start + region_width > dst_grid.imt ||
        dst_j_start < 0 || dst_j_start + region_height > dst_grid.jmt ||
        dst_k_start < 0 || dst_k_start + region_depth > dst_grid.kmt) {
        DEBUG_ERROR("Destination region would be out of bounds in add_interface");
        return;
    }
    
    // Add the interface
    interfaces.emplace_back(
        src_cavity_id, dst_cavity_id,
        src_i_start, src_i_end, src_j_start, src_j_end, src_k_start, src_k_end,
        dst_i_start, dst_j_start, dst_k_start
    );
    
    DEBUG_INFO("Added interface: Cavity " << src_cavity_id << " -> Cavity " << dst_cavity_id
              << " with region size " << region_width << "x" << region_height << "x" << region_depth);
}

void MultiCavityMatrixBuilder::global_to_local(int global_idx, int& cavity_id, int& local_idx) const {
    if (global_idx < 0 || global_idx >= total_grid_points) {
        DEBUG_ERROR("Global index " << global_idx << " out of range");
        cavity_id = -1;
        local_idx = -1;
        return;
    }
    
    // Binary search to find the cavity containing the global index
    int low = 0;
    int high = static_cast<int>(cavity_grids.size()) - 1;
    while (low <= high) {
        int mid = low + (high - low) / 2;
        int offset = cavity_offsets[mid];
        int next_offset = (mid + 1 < static_cast<int>(cavity_grids.size())) 
                          ? cavity_offsets[mid + 1] : total_grid_points;
        
        if (global_idx >= offset && global_idx < next_offset) {
            cavity_id = mid;
            local_idx = global_idx - offset;
            return;
        } else if (global_idx < offset) {
            high = mid - 1;
        } else {
            low = mid + 1;
        }
    }
    
    // Index not found
    DEBUG_ERROR("Global index " << global_idx << " is out of range");
    cavity_id = -1;
    local_idx = -1;
}

void MultiCavityMatrixBuilder::global_to_local_3d(int global_idx, int& cavity_id, int& i, int& j, int& k) const {
    // First convert to cavity + local index
    int local_idx;
    global_to_local(global_idx, cavity_id, local_idx);
    
    if (cavity_id < 0) {
        i = j = k = -1;
        return;
    }
    
    // Convert local index to 3D coordinates
    const Grid3D& grid = cavity_grids[cavity_id];
    k = local_idx / (grid.imt * grid.jmt);
    int remainder = local_idx % (grid.imt * grid.jmt);
    j = remainder / grid.imt;
    i = remainder % grid.imt;
}

int MultiCavityMatrixBuilder::local_to_global(int cavity_id, int local_idx) const {
    if (cavity_id < 0 || cavity_id >= static_cast<int>(cavity_grids.size())) {
        DEBUG_ERROR("Invalid cavity ID: " << cavity_id);
        return -1;
    }
    
    // Validate local index
    const Grid3D& grid = cavity_grids[cavity_id];
    if (local_idx < 0 || local_idx >= grid.total_points()) {
        DEBUG_ERROR("Local index " << local_idx << " is out of range for cavity " << cavity_id);
        return -1;
    }
    
    return cavity_offsets[cavity_id] + local_idx;
}

int MultiCavityMatrixBuilder::local_to_global_3d(int cavity_id, int i, int j, int k) const {
    if (cavity_id < 0 || cavity_id >= static_cast<int>(cavity_grids.size())) {
        DEBUG_ERROR("Invalid cavity ID: " << cavity_id);
        return -1;
    }
    
    // Validate 3D coordinates
    const Grid3D& grid = cavity_grids[cavity_id];
    if (i < 0 || i >= grid.imt ||
        j < 0 || j >= grid.jmt ||
        k < 0 || k >= grid.kmt) {
        DEBUG_ERROR("3D coordinates (" << i << ", " << j << ", " << k 
                   << ") are out of bounds for cavity " << cavity_id);
        return -1;
    }
    
    // Calculate local index
    int local_idx = i + j * grid.imt + k * grid.imt * grid.jmt;
    
    return cavity_offsets[cavity_id] + local_idx;
}

// ------------------------------------------------------------------------
// MultiCavityMatrixBuilder Implementation - Matrix Building
// ------------------------------------------------------------------------

void MultiCavityMatrixBuilder::initialize_indexing() {
    cavity_offsets.resize(cavity_grids.size() + 1, 0);
    
    // Calculate total grid points and cavity offsets
    total_grid_points = 0;
    for (int i = 0; i < static_cast<int>(cavity_grids.size()); i++) {
        cavity_offsets[i] = total_grid_points;
        total_grid_points += cavity_grids[i].total_points();
    }
    cavity_offsets[cavity_grids.size()] = total_grid_points;
    
    DEBUG_INFO("Initialized indexing with " << total_grid_points << " total grid points");
}

void MultiCavityMatrixBuilder::validate_interfaces() const {
    for (int i = 0; i < static_cast<int>(interfaces.size()); i++) {
        const auto& interface = interfaces[i];
        
        if (interface.src_cavity_id < 0 || interface.src_cavity_id >= static_cast<int>(cavity_grids.size()) ||
            interface.dst_cavity_id < 0 || interface.dst_cavity_id >= static_cast<int>(cavity_grids.size())) {
            DEBUG_ERROR("Interface " << i << " has invalid cavity IDs: " 
                      << interface.src_cavity_id << " -> " << interface.dst_cavity_id);
            continue;
        }
        
        const Grid3D& src_grid = cavity_grids[interface.src_cavity_id];
        const Grid3D& dst_grid = cavity_grids[interface.dst_cavity_id];
        
        // Validate source region bounds
        if (interface.src_i_end >= src_grid.imt || interface.src_i_start < 0 ||
            interface.src_j_end >= src_grid.jmt || interface.src_j_start < 0 ||
            interface.src_k_end >= src_grid.kmt || interface.src_k_start < 0) {
            DEBUG_ERROR("Interface source region is out of bounds");
        }
        
        // Calculate region dimensions
        int width = interface.src_i_end - interface.src_i_start + 1;
        int height = interface.src_j_end - interface.src_j_start + 1;
        int depth = interface.src_k_end - interface.src_k_start + 1;
        
        // Validate destination region bounds
        if (interface.dst_i_start < 0 || interface.dst_i_start + width > dst_grid.imt ||
            interface.dst_j_start < 0 || interface.dst_j_start + height > dst_grid.jmt ||
            interface.dst_k_start < 0 || interface.dst_k_start + depth > dst_grid.kmt) {
            DEBUG_ERROR("Interface destination region is out of bounds");
        }
    }
}

// ------------------------------------------------------------------------
// Core Matrix Building Functions
// ------------------------------------------------------------------------

float* MultiCavityMatrixBuilder::build_coefficient_matrix() {
    // Validate the configuration
    if (cavity_grids.empty()) {
        DEBUG_ERROR("No cavities defined");
        return nullptr;
    }
    
    validate_interfaces();
    
    // Number of diagonals is based on the method
    switch (method) {
        case DiscretizationMethod::FDM:
            num_diagonals = 19;  // 19-point stencil
            break;
        case DiscretizationMethod::SEM:
        case DiscretizationMethod::SPECTRAL:
            num_diagonals = 64;  // For spectral methods
            break;
        default:
            DEBUG_ERROR("Unknown discretization method");
            return nullptr;
    }
    
    // Initialize global indexing if not already done
    if (cavity_offsets.empty() || cavity_offsets.size() != cavity_grids.size() + 1) {
        initialize_indexing();
    }
    
    // Compute total size of the coefficient tensor
    int total_size = 0;
    for (const auto& grid : cavity_grids) {
        total_size += num_diagonals * grid.total_points();
    }
    
    // Allocate the tensor data
    float* tensor_data = new float[total_size]();  // Initialize to zeros
    DEBUG_INFO("Allocated coefficient tensor with " << total_size << " elements");
    
    // Build operators for each cavity
    for (int cavity_id = 0; cavity_id < cavity_grids.size(); cavity_id++) {
        DEBUG_INFO("Building operator for cavity " << cavity_id);
        build_cavity_operator(cavity_id, tensor_data);
    }
    
    // Connect cavities at interfaces
    DEBUG_INFO("Connecting cavities at interfaces");
    connect_cavities(tensor_data);
    
    // Apply boundary conditions
    DEBUG_INFO("Applying boundary conditions");
    apply_boundary_conditions(tensor_data);
    
    // Print debug info about the constructed tensor
    if (DEBUG_LEVEL >= 3) {
        print_tensor_stats(tensor_data);
    }
    
    return tensor_data;
}

void MultiCavityMatrixBuilder::build_cavity_operator(int cavity_id, float* tensor_data) {
    const Grid3D& grid = cavity_grids[cavity_id];
    
    // Get the base offset for this cavity in the tensor_data array
    int cavity_tensor_offset = 0;
    for (int i = 0; i < cavity_id; i++) {
        cavity_tensor_offset += num_diagonals * cavity_grids[i].total_points();
    }
    
    // Get the grid dimensions
    int imt = grid.imt;
    int jmt = grid.jmt;
    int kmt = grid.kmt;
    
    // Calculate grid spacing
    double dx = (grid.x_max - grid.x_min) / (imt - 1);
    double dy = (grid.y_max - grid.y_min) / (jmt - 1);
    double dz = (grid.z_max - grid.z_min) / (kmt - 1);
    
    // Debug the grid spacing
    DEBUG_INFO("Cavity " << cavity_id << " grid spacing: dx=" << dx
              << ", dy=" << dy << ", dz=" << dz);
    
    // For FDM, build a 19-point stencil operator
    if (method == DiscretizationMethod::FDM) {
        // Coefficients for the 3D Laplacian operator using FDM
        // The 19-point stencil consists of:
        // - The center point (i,j,k)
        // - 6 direct neighbors (i±1,j,k), (i,j±1,k), (i,j,k±1)
        // - 12 diagonal neighbors (i±1,j±1,k), (i±1,j,k±1), (i,j±1,k±1)
        
        // Constants for the discrete Laplacian
        double inv_dx2 = 1.0 / (dx * dx);
        double inv_dy2 = 1.0 / (dy * dy);
        double inv_dz2 = 1.0 / (dz * dz);
        
        // Iterate over each interior point in the cavity
        for (int k = 0; k < kmt; k++) {
            for (int j = 0; j < jmt; j++) {
                for (int i = 0; i < imt; i++) {
                    // Index in the flattened grid
                    int local_idx = i + j * imt + k * imt * jmt;
                    
                    // Base index for this point in the tensor_data array
                    int tensor_idx = cavity_tensor_offset + local_idx * num_diagonals;
                    
                    // Check if this is an interior point or a boundary point
                    bool is_interior = (i > 0 && i < imt-1 && 
                                       j > 0 && j < jmt-1 && 
                                       k > 0 && k < kmt-1);
                    
                    if (is_interior) {
                        // Interior point - set up the Laplacian stencil
                        
                        // Center point coefficient (diagonal term)
                        // For the standard 7-point stencil in 3D, this is -6 * (1/dx^2 + 1/dy^2 + 1/dz^2)
                        tensor_data[tensor_idx + 0] = -2.0 * (inv_dx2 + inv_dy2 + inv_dz2);
                        
                        // The six direct neighbors
                        // i+1,j,k
                        tensor_data[tensor_idx + 1] = inv_dx2;
                        // i-1,j,k
                        tensor_data[tensor_idx + 2] = inv_dx2;
                        // i,j+1,k
                        tensor_data[tensor_idx + 3] = inv_dy2;
                        // i,j-1,k
                        tensor_data[tensor_idx + 4] = inv_dy2;
                        // i,j,k+1
                        tensor_data[tensor_idx + 5] = inv_dz2;
                        // i,j,k-1
                        tensor_data[tensor_idx + 6] = inv_dz2;
                        
                        // The twelve diagonal neighbors (these might need adjustment for your specific stencil)
                        // We're assigning values for a 19-point stencil
                        // i+1,j+1,k
                        tensor_data[tensor_idx + 7] = 0.0;
                        // i+1,j-1,k
                        tensor_data[tensor_idx + 8] = 0.0;
                        // i-1,j+1,k
                        tensor_data[tensor_idx + 9] = 0.0;
                        // i-1,j-1,k
                        tensor_data[tensor_idx + 10] = 0.0;
                        // i+1,j,k+1
                        tensor_data[tensor_idx + 11] = 0.0;
                        // i+1,j,k-1
                        tensor_data[tensor_idx + 12] = 0.0;
                        // i-1,j,k+1
                        tensor_data[tensor_idx + 13] = 0.0;
                        // i-1,j,k-1
                        tensor_data[tensor_idx + 14] = 0.0;
                        // i,j+1,k+1
                        tensor_data[tensor_idx + 15] = 0.0;
                        // i,j+1,k-1
                        tensor_data[tensor_idx + 16] = 0.0;
                        // i,j-1,k+1
                        tensor_data[tensor_idx + 17] = 0.0;
                        // i,j-1,k-1
                        tensor_data[tensor_idx + 18] = 0.0;
                    } else {
                        // Boundary point - identity matrix (diagonal=1)
                        // This will be overridden for interface points later
                        tensor_data[tensor_idx + 0] = 1.0;  // diagonal term
                    }
                }
            }
        }
    }
    else if (method == DiscretizationMethod::SEM || method == DiscretizationMethod::SPECTRAL) {
        // For Spectral/SEM methods, we implement a proper discretization
        DEBUG_INFO("Implementing " << 
                  (method == DiscretizationMethod::SEM ? "SEM" : "Spectral") << 
                  " discretization for cavity " << cavity_id);
        
        // Determine the method type and parameters
        int N = (method == DiscretizationMethod::SEM) ? 
                std::max(2, std::min(imt-1, std::min(jmt-1, kmt-1)) / 2) : // SEM uses polynomial order
                std::max(2, std::min(imt-1, std::min(jmt-1, kmt-1)));      // Spectral uses total points
        
        // For SEM, calculate the number of elements in each direction
        int Ncell_x = (method == DiscretizationMethod::SEM) ? std::max(1, (imt - 1) / N) : 1;
        int Ncell_y = (method == DiscretizationMethod::SEM) ? std::max(1, (jmt - 1) / N) : 1;
        int Ncell_z = (method == DiscretizationMethod::SEM) ? std::max(1, (kmt - 1) / N) : 1;
        
        // Domain half-lengths
        double Lx = (grid.x_max - grid.x_min) / 2.0;
        double Ly = (grid.y_max - grid.y_min) / 2.0;
        double Lz = (grid.z_max - grid.z_min) / 2.0;
        
        DEBUG_INFO("Using discretization parameters: N=" << N << 
                  ", Ncell_x=" << Ncell_x << ", Ncell_y=" << Ncell_y << 
                  ", Ncell_z=" << Ncell_z);
        
        // Generate 1D operators for each direction
        std::vector<double> x_points, y_points, z_points;
        std::vector<std::vector<double>> D1x, D2x;
        std::vector<std::vector<double>> D1y, D2y;
        std::vector<std::vector<double>> D1z, D2z;
        
        try {
            // For SEM, we need to adjust N to ensure the generated points match the grid dimensions
            // The number of points in SEM is N*Ncell + 1, so we solve for N
            if (method == DiscretizationMethod::SEM) {
                // Calculate N to match grid dimensions exactly
                // For SEM, the number of points is N*Ncell + 1, so we need to solve for N
                // N = (grid_size - 1) / Ncell
                
                // For cavity 0 (10x10x10), we need N = (10-1)/2 = 4.5, which rounds to 4
                // This gives 4*2+1 = 9 points, not 10
                // For cavity 1 (8x8x8), we need N = (8-1)/2 = 3.5, which rounds to 3
                // This gives 3*2+1 = 7 points, not 8
                
                // To get exactly 10 points with 2 elements, we need N = 4.5
                // To get exactly 8 points with 2 elements, we need N = 3.5
                // Since N must be an integer, we'll use a different approach:
                
                // Adjust the number of cells to match the grid dimensions
                // For a grid of size M, we want N*Ncell + 1 = M
                // If we fix N=2 (minimum for SEM), then Ncell = (M-1)/N
                
                int N_fixed = 3; // Use a fixed polynomial order
                int Ncell_x = (imt - 1 + N_fixed - 1) / N_fixed; // Ceiling division to ensure enough points
                int Ncell_y = (jmt - 1 + N_fixed - 1) / N_fixed;
                int Ncell_z = (kmt - 1 + N_fixed - 1) / N_fixed;
                
                // Now calculate the exact N needed for each direction
                int N_x = (imt - 1) / Ncell_x;
                int N_y = (jmt - 1) / Ncell_y;
                int N_z = (kmt - 1) / Ncell_z;
                
                // Ensure N is at least 2 for each direction
                N_x = std::max(2, N_x);
                N_y = std::max(2, N_y);
                N_z = std::max(2, N_z);
                
                // Calculate the expected number of points
                int expected_x_points = N_x * Ncell_x + 1;
                int expected_y_points = N_y * Ncell_y + 1;
                int expected_z_points = N_z * Ncell_z + 1;
                
                DEBUG_INFO("Expected points: x=" << expected_x_points << 
                          ", y=" << expected_y_points << ", z=" << expected_z_points);
                
                // If the expected points don't match the grid dimensions, adjust N
                if (expected_x_points != imt) {
                    // We need to adjust N_x or Ncell_x
                    if (expected_x_points < imt) {
                        // Increase N_x
                        N_x++;
                    } else {
                        // Decrease N_x if possible
                        N_x = std::max(2, N_x - 1);
                    }
                    expected_x_points = N_x * Ncell_x + 1;
                }
                
                if (expected_y_points != jmt) {
                    // We need to adjust N_y or Ncell_y
                    if (expected_y_points < jmt) {
                        // Increase N_y
                        N_y++;
                    } else {
                        // Decrease N_y if possible
                        N_y = std::max(2, N_y - 1);
                    }
                    expected_y_points = N_y * Ncell_y + 1;
                }
                
                if (expected_z_points != kmt) {
                    // We need to adjust N_z or Ncell_z
                    if (expected_z_points < kmt) {
                        // Increase N_z
                        N_z++;
                    } else {
                        // Decrease N_z if possible
                        N_z = std::max(2, N_z - 1);
                    }
                    expected_z_points = N_z * Ncell_z + 1;
                }
                
                DEBUG_INFO("Adjusted expected points: x=" << expected_x_points << 
                          ", y=" << expected_y_points << ", z=" << expected_z_points);
                
                // Generate the appropriate operators based on method
                DEBUG_INFO("SEM Operator Generation: N=" << N_x << ", Ncell=" << Ncell_x << ", L=" << Lx);
                generate_sem_operators_1d(N_x, Ncell_x, Lx, x_points, D1x, D2x);
                DEBUG_INFO("SEM Operators Generated: points.size()=" << x_points.size() << 
                          ", D1.size()=" << D1x.size() << ", D2.size()=" << D2x.size());
                
                DEBUG_INFO("SEM Operator Generation: N=" << N_y << ", Ncell=" << Ncell_y << ", L=" << Ly);
                generate_sem_operators_1d(N_y, Ncell_y, Ly, y_points, D1y, D2y);
                generate_sem_operators_1d(N_y, Ncell_y, Ly, y_points, D1y, D2y);
                DEBUG_INFO("SEM Operators Generated: points.size()=" << y_points.size() << 
                          ", D1.size()=" << D1y.size() << ", D2.size()=" << D2y.size());
                
                DEBUG_INFO("SEM Operator Generation: N=" << N_z << ", Ncell=" << Ncell_z << ", L=" << Lz);
                generate_sem_operators_1d(N_z, Ncell_z, Lz, z_points, D1z, D2z);
                DEBUG_INFO("SEM Operators Generated: points.size()=" << z_points.size() << 
                          ", D1.size()=" << D1z.size() << ", D2.size()=" << D2z.size());
            } else {
                // Spectral uses Chebyshev points for the entire domain
                DEBUG_INFO("Generating Spectral operators with N=" << N);
                generate_chebyshev_operators(N, Lx, x_points, D1x, D2x);
                generate_chebyshev_operators(N, Ly, y_points, D1y, D2y);
                generate_chebyshev_operators(N, Lz, z_points, D1z, D2z);
            }
        } catch (const std::exception& e) {
            DEBUG_ERROR("Exception during operator generation: " << e.what());
            // Fill with identity matrix as fallback
            for (int k = 0; k < kmt; k++) {
                for (int j = 0; j < jmt; j++) {
                    for (int i = 0; i < imt; i++) {
                        int local_idx = i + j * imt + k * imt * jmt;
                        int tensor_idx = cavity_tensor_offset + local_idx * num_diagonals;
                        tensor_data[tensor_idx] = 1.0f;  // Identity matrix
                    }
                }
            }
            return;
        }
        
        // Check if the generated points match the grid dimensions
        if (x_points.size() != imt || y_points.size() != jmt || z_points.size() != kmt) {
            DEBUG_ERROR("[DEBUG][L1] ERROR: Generated points do not match grid dimensions: " 
                      << "x_points.size()=" << x_points.size() << " (expected " << imt << "), "
                      << "y_points.size()=" << y_points.size() << " (expected " << jmt << "), "
                      << "z_points.size()=" << z_points.size() << " (expected " << kmt << ")");
            
            // For SEM/Spectral methods, we'll adapt the grid to match the generated points
            // This is a better approach than trying to force the spectral points to match a uniform grid
            if (method == DiscretizationMethod::SEM || method == DiscretizationMethod::SPECTRAL) {
                DEBUG_INFO("Adapting grid dimensions to match spectral points");
                
                // Update the grid dimensions to match the spectral points
                Grid3D& mutable_grid = cavity_grids[cavity_id];
                mutable_grid.imt = static_cast<int>(x_points.size());
                mutable_grid.jmt = static_cast<int>(y_points.size());
                mutable_grid.kmt = static_cast<int>(z_points.size());
                
                // Update local variables
                imt = mutable_grid.imt;
                jmt = mutable_grid.jmt;
                kmt = mutable_grid.kmt;
                
                DEBUG_INFO("Updated grid dimensions: imt=" << imt << ", jmt=" << jmt << ", kmt=" << kmt);
            } else {
                // For other methods, resize the matrices to match the grid dimensions
                if (x_points.size() < imt) {
                    DEBUG_WARN("Resizing x_points and D2x to match grid dimensions");
                    x_points.resize(imt);
                    D2x.resize(imt, std::vector<double>(imt, 0.0));
                }
                if (y_points.size() < jmt) {
                    DEBUG_WARN("Resizing y_points and D2y to match grid dimensions");
                    y_points.resize(jmt);
                    D2y.resize(jmt, std::vector<double>(jmt, 0.0));
                }
                if (z_points.size() < kmt) {
                    DEBUG_WARN("Resizing z_points and D2z to match grid dimensions");
                    z_points.resize(kmt);
                    D2z.resize(kmt, std::vector<double>(kmt, 0.0));
                }
            }
        }
        
        // Calculate points in spectral space
        // SEM and Spectral methods use non-uniform grid spacing
        std::vector<double> x_grid(imt), y_grid(jmt), z_grid(kmt);
        
        // Map the spectral points to the physical domain
        for (int i = 0; i < imt && i < x_points.size(); i++) {
            x_grid[i] = grid.x_min + (x_points[i] + Lx);
        }
        
        for (int j = 0; j < jmt && j < y_points.size(); j++) {
            y_grid[j] = grid.y_min + (y_points[j] + Ly);
        }
        
        for (int k = 0; k < kmt && k < z_points.size(); k++) {
            z_grid[k] = grid.z_min + (z_points[k] + Lz);
        }
        
        // Debug grid points
        if (DEBUG_LEVEL >= 4) {
            DEBUG_DETAIL("X grid points:");
            for (int i = 0; i < imt; i++) {
                DEBUG_DETAIL(i << ": " << x_grid[i]);
            }
        }
        
        // Build the discrete Laplacian operator using tensor product
        // The Laplacian is D2x ⊗ Iy ⊗ Iz + Ix ⊗ D2y ⊗ Iz + Ix ⊗ Iy ⊗ D2z
        DEBUG_INFO("Building 3D Laplacian operator using tensor product");
        
        // Iterate over each point in the cavity
        for (int k = 0; k < kmt; k++) {
            for (int j = 0; j < jmt; j++) {
                for (int i = 0; i < imt; i++) {
                    // Index in the flattened grid
                    int local_idx = i + j * imt + k * imt * jmt;
                    
                    // Base index for this point in the tensor_data array
                    int tensor_idx = cavity_tensor_offset + local_idx * num_diagonals;
                    
                    // Initialize all stencil entries to zero
                    for (int stencil_idx = 0; stencil_idx < num_diagonals; stencil_idx++) {
                        tensor_data[tensor_idx + stencil_idx] = 0.0f;
                    }
                    
                    // Set the diagonal term (center point)
                    double diag_term = 0.0;
                    if (i < D2x.size() && i < D2x[i].size()) diag_term += D2x[i][i];
                    if (j < D2y.size() && j < D2y[j].size()) diag_term += D2y[j][j];
                    if (k < D2z.size() && k < D2z[k].size()) diag_term += D2z[k][k];
                    
                    // Ensure diagonal term is non-zero for numerical stability
                    if (std::abs(diag_term) < 1e-10) {
                        diag_term = 1.0;  // Use identity matrix as fallback
                    }
                    
                    tensor_data[tensor_idx + 0] = static_cast<float>(diag_term);
                    
                    // Apply the tensor product for the discretized Laplacian
                    int stencil_idx = 1; // Start from the first off-diagonal element
                    
                    // Apply D2x terms (x-direction second derivatives)
                    for (int ii = 0; ii < imt && stencil_idx < num_diagonals && i < D2x.size() && ii < D2x[i].size(); ii++) {
                        if (ii != i && std::abs(D2x[i][ii]) > 1e-10) {
                            tensor_data[tensor_idx + stencil_idx] = static_cast<float>(D2x[i][ii]);
                            stencil_idx++;
                        }
                    }
                    
                    // Apply D2y terms (y-direction second derivatives)
                    for (int jj = 0; jj < jmt && stencil_idx < num_diagonals && j < D2y.size() && jj < D2y[j].size(); jj++) {
                        if (jj != j && std::abs(D2y[j][jj]) > 1e-10) {
                            tensor_data[tensor_idx + stencil_idx] = static_cast<float>(D2y[j][jj]);
                            stencil_idx++;
                        }
                    }
                    
                    // Apply D2z terms (z-direction second derivatives)
                    for (int kk = 0; kk < kmt && stencil_idx < num_diagonals && k < D2z.size() && kk < D2z[k].size(); kk++) {
                        if (kk != k && std::abs(D2z[k][kk]) > 1e-10) {
                            tensor_data[tensor_idx + stencil_idx] = static_cast<float>(D2z[k][kk]);
                            stencil_idx++;
                        }
                    }
                    
                    // Fill unused stencil entries with zeros
                    for (; stencil_idx < num_diagonals; stencil_idx++) {
                        tensor_data[tensor_idx + stencil_idx] = 0.0f;
                    }
                }
            }
        }
    }
    
    DEBUG_INFO("Built operator for cavity " << cavity_id);
}

void MultiCavityMatrixBuilder::connect_cavities(float* tensor_data) {
    // Process each interface
    for (const auto& interface : interfaces) {
        int src_cavity_id = interface.src_cavity_id;
        int dst_cavity_id = interface.dst_cavity_id;
        
        const Grid3D& src_grid = cavity_grids[src_cavity_id];
        const Grid3D& dst_grid = cavity_grids[dst_cavity_id];
        
        // Calculate tensor offsets for each cavity
        int src_tensor_offset = 0;
        int dst_tensor_offset = 0;
        
        for (int i = 0; i < src_cavity_id; i++) {
            src_tensor_offset += num_diagonals * cavity_grids[i].total_points();
        }
        
        for (int i = 0; i < dst_cavity_id; i++) {
            dst_tensor_offset += num_diagonals * cavity_grids[i].total_points();
        }
        
        DEBUG_INFO("Connecting interface: Cavity " << src_cavity_id 
                  << " -> Cavity " << dst_cavity_id);
        
        // Loop over the interface region in the source cavity
        for (int k = interface.src_k_start; k <= interface.src_k_end; k++) {
            for (int j = interface.src_j_start; j <= interface.src_j_end; j++) {
                for (int i = interface.src_i_start; i <= interface.src_i_end; i++) {
                    // Calculate source cavity local index
                    int src_local_idx = i + j * src_grid.imt + k * src_grid.imt * src_grid.jmt;
                    
                    // Calculate corresponding destination point
                    int dst_i = interface.dst_i_start + (i - interface.src_i_start);
                    int dst_j = interface.dst_j_start + (j - interface.src_j_start);
                    int dst_k = interface.dst_k_start + (k - interface.src_k_start);
                    
                    int dst_local_idx = dst_i + dst_j * dst_grid.imt + dst_k * dst_grid.imt * dst_grid.jmt;
                    
                    // Base indices in the tensor data for both points
                    int src_tensor_idx = src_tensor_offset + src_local_idx * num_diagonals;
                    int dst_tensor_idx = dst_tensor_offset + dst_local_idx * num_diagonals;
                    
                    // Connect the points by setting the diagonal elements
                    // For the interface points, we'll use a simple approach:
                    // Set the source point diagonal to 0.5 and add a connection to the destination point
                    // Set the destination point diagonal to 0.5 and add a connection to the source point
                    
                    // Modify source point
                    tensor_data[src_tensor_idx] = 0.5;  // Diagonal term
                    
                    // Modify destination point
                    tensor_data[dst_tensor_idx] = 0.5;  // Diagonal term
                    
                    // In a more advanced implementation, you would need to adjust the stencil
                    // at the interface points based on the discretization method
                    
                    DEBUG_DETAIL("Connected points: Src(" << i << "," << j << "," << k 
                               << ") -> Dst(" << dst_i << "," << dst_j << "," << dst_k << ")");
                }
            }
        }
    }
    
    DEBUG_INFO("Connected " << interfaces.size() << " cavity interfaces");
}

void MultiCavityMatrixBuilder::apply_boundary_conditions(float* tensor_data) {
    // Apply Dirichlet boundary conditions (u = 0 at the boundary)
    // This means setting the diagonal element to 1 and all other elements to 0
    
    for (int cavity_id = 0; cavity_id < cavity_grids.size(); cavity_id++) {
        const Grid3D& grid = cavity_grids[cavity_id];
        
        // Calculate tensor offset for this cavity
        int tensor_offset = 0;
        for (int i = 0; i < cavity_id; i++) {
            tensor_offset += num_diagonals * cavity_grids[i].total_points();
        }
        
        // Handle all boundary points that are not on interfaces
        for (int k = 0; k < grid.kmt; k++) {
            for (int j = 0; j < grid.jmt; j++) {
                for (int i = 0; i < grid.imt; i++) {
                    // Check if this is a boundary point
                    bool is_boundary = (i == 0 || i == grid.imt-1 || 
                                       j == 0 || j == grid.jmt-1 || 
                                       k == 0 || k == grid.kmt-1);
                    
                    if (is_boundary) {
                        // Need to check if this point is on an interface
                        bool is_on_interface = false;
                        
                        // Check each interface
                        for (const auto& interface : interfaces) {
                            if (interface.src_cavity_id == cavity_id) {
                                // Check if point is in the source region of this interface
                                if (i >= interface.src_i_start && i <= interface.src_i_end &&
                                    j >= interface.src_j_start && j <= interface.src_j_end &&
                                    k >= interface.src_k_start && k <= interface.src_k_end) {
                                    is_on_interface = true;
                                    break;
                                }
                            }
                            
                            if (interface.dst_cavity_id == cavity_id) {
                                // Check if point is in the destination region of this interface
                                int rel_i = i - interface.dst_i_start;
                                int rel_j = j - interface.dst_j_start;
                                int rel_k = k - interface.dst_k_start;
                                
                                if (rel_i >= 0 && rel_i <= (interface.src_i_end - interface.src_i_start) &&
                                    rel_j >= 0 && rel_j <= (interface.src_j_end - interface.src_j_start) &&
                                    rel_k >= 0 && rel_k <= (interface.src_k_end - interface.src_k_start)) {
                                    is_on_interface = true;
                                    break;
                                }
                            }
                        }
                        
                        // Apply boundary condition if this is not an interface point
                        if (!is_on_interface) {
                            int local_idx = i + j * grid.imt + k * grid.imt * grid.jmt;
                            int tensor_idx = tensor_offset + local_idx * num_diagonals;
                            
                            // Set diagonal to 1 (identity)
                            tensor_data[tensor_idx] = 1.0;
                            
                            // Clear all other entries for this row
                            for (int m = 1; m < num_diagonals; m++) {
                                tensor_data[tensor_idx + m] = 0.0;
                            }
                            
                            DEBUG_DETAIL("Applied Dirichlet BC at cavity " << cavity_id 
                                       << " point (" << i << "," << j << "," << k << ")");
                        }
                    }
                }
            }
        }
    }
    
    DEBUG_INFO("Applied boundary conditions");
}

void MultiCavityMatrixBuilder::print_tensor_stats(const float* tensor_data) const {
    // Print some statistics about the tensor data to verify correctness
    
    // Count number of non-zeros
    int total_elements = 0;
    int nonzero_elements = 0;
    
    for (int cavity_id = 0; cavity_id < cavity_grids.size(); cavity_id++) {
        const Grid3D& grid = cavity_grids[cavity_id];
        int cavity_points = grid.total_points();
        
        // Calculate tensor offset for this cavity
        int tensor_offset = 0;
        for (int i = 0; i < cavity_id; i++) {
            tensor_offset += num_diagonals * cavity_grids[i].total_points();
        }
        
        // Count nonzero elements for this cavity
        for (int pt = 0; pt < cavity_points; pt++) {
            for (int m = 0; m < num_diagonals; m++) {
                int idx = tensor_offset + pt * num_diagonals + m;
                total_elements++;
                if (tensor_data[idx] != 0.0f) {
                    nonzero_elements++;
                }
            }
        }
    }
    
    double sparsity = 100.0 * (1.0 - static_cast<double>(nonzero_elements) / total_elements);
    
    std::cout << "Tensor statistics:" << std::endl;
    std::cout << "  Total elements: " << total_elements << std::endl;
    std::cout << "  Non-zero elements: " << nonzero_elements << std::endl;
    std::cout << "  Sparsity: " << std::fixed << std::setprecision(2) << sparsity << "%" << std::endl;
}

void MultiCavityMatrixBuilder::print_info() const {
    std::cout << "===== MultiCavityMatrixBuilder Information =====" << std::endl;
    std::cout << "Discretization method: " << method_to_string(method) << std::endl;
    std::cout << "Number of diagonals: " << num_diagonals << std::endl;
    std::cout << "Number of cavities: " << cavity_grids.size() << std::endl;
    std::cout << "Total grid points: " << total_grid_points << std::endl;
    
    std::cout << "\n--- Cavities ---" << std::endl;
    for (int i = 0; i < cavity_grids.size(); i++) {
        std::cout << "Cavity " << i << " (offset: " << cavity_offsets[i] << "):" << std::endl;
        cavity_grids[i].print_info();
    }
    
    std::cout << "\n--- Interfaces ---" << std::endl;
    for (int i = 0; i < interfaces.size(); i++) {
        std::cout << "Interface " << i << ":" << std::endl;
        interfaces[i].print_info();
    }
    
    std::cout << "=============================================" << std::endl;
}

// ------------------------------------------------------------------------
// Utility Functions Implementation
// ------------------------------------------------------------------------

void dump_tensor_to_file(const float* tensor_data, int num_cavities, int num_diags, 
                         const std::vector<Grid3D>& grids, const std::string& filename) {
    std::ofstream file(filename, std::ios::binary);
    if (!file) {
        std::cerr << "Failed to open file for writing: " << filename << std::endl;
        return;
    }
    
    // Calculate total elements
    int total_elements = 0;
    for (const auto& grid : grids) {
        total_elements += num_diags * grid.total_points();
    }
    
    // Write header information
    file.write(reinterpret_cast<const char*>(&num_cavities), sizeof(int));
    file.write(reinterpret_cast<const char*>(&num_diags), sizeof(int));
    file.write(reinterpret_cast<const char*>(&total_elements), sizeof(int));
    
    // Write each cavity's dimensions
    for (const auto& grid : grids) {
        file.write(reinterpret_cast<const char*>(&grid.imt), sizeof(int));
        file.write(reinterpret_cast<const char*>(&grid.jmt), sizeof(int));
        file.write(reinterpret_cast<const char*>(&grid.kmt), sizeof(int));
    }
    
    // Write the tensor data
    file.write(reinterpret_cast<const char*>(tensor_data), total_elements * sizeof(float));
    
    file.close();
    std::cout << "Dumped tensor data to file: " << filename << std::endl;
    std::cout << "  Num cavities: " << num_cavities << std::endl;
    std::cout << "  Num diagonals: " << num_diags << std::endl;
    std::cout << "  Total elements: " << total_elements << std::endl;
}

// ------------------------------------------------------------------------
// Matrix Operations Implementation
// ------------------------------------------------------------------------

void MultiCavityMatrixBuilder::matrix_vector_multiply(const float* tensor_data, const float* x, float* result) const {
    // Perform y = A*x for the multi-cavity coefficient matrix
    
    // Initialize result vector to zero
    std::memset(result, 0, total_grid_points * sizeof(float));
    
    // Process each cavity
    for (int cavity_id = 0; cavity_id < cavity_grids.size(); cavity_id++) {
        const Grid3D& grid = cavity_grids[cavity_id];
        
        // Calculate tensor offset for this cavity
        int tensor_offset = 0;
        for (int i = 0; i < cavity_id; i++) {
            tensor_offset += num_diagonals * cavity_grids[i].total_points();
        }
        
        // Calculate the index offset for this cavity in the global vectors
        int global_offset = cavity_offsets[cavity_id];
        
        DEBUG_DETAIL("MatVec for cavity " << cavity_id << " (offset: " << global_offset << ")");
        
        // Loop over each point in the cavity
        for (int k = 0; k < grid.kmt; k++) {
            for (int j = 0; j < grid.jmt; j++) {
                for (int i = 0; i < grid.imt; i++) {
                    // Local index within this cavity
                    int local_idx = i + j * grid.imt + k * grid.imt * grid.jmt;
                    
                    // Global index
                    int global_idx = global_offset + local_idx;
                    
                    // Base index in the tensor data for this point
                    int tensor_idx = tensor_offset + local_idx * num_diagonals;
                    
                    // Process diagonal term (center point)
                    result[global_idx] += tensor_data[tensor_idx] * x[global_idx];
                    
                    // Process off-diagonal terms for FDM 19-point stencil
                    if (method == DiscretizationMethod::FDM) {
                        // For interior points, apply the stencil
                        if (i > 0 && i < grid.imt-1 && j > 0 && j < grid.jmt-1 && k > 0 && k < grid.kmt-1) {
                            // The six direct neighbors
                            // i+1,j,k
                            if (i+1 < grid.imt) {
                                int neighbor_local = (i+1) + j * grid.imt + k * grid.imt * grid.jmt;
                                int neighbor_global = global_offset + neighbor_local;
                                result[global_idx] += tensor_data[tensor_idx + 1] * x[neighbor_global];
                            }
                            
                            // i-1,j,k
                            if (i-1 >= 0) {
                                int neighbor_local = (i-1) + j * grid.imt + k * grid.imt * grid.jmt;
                                int neighbor_global = global_offset + neighbor_local;
                                result[global_idx] += tensor_data[tensor_idx + 2] * x[neighbor_global];
                            }
                            
                            // i,j+1,k
                            if (j+1 < grid.jmt) {
                                int neighbor_local = i + (j+1) * grid.imt + k * grid.imt * grid.jmt;
                                int neighbor_global = global_offset + neighbor_local;
                                result[global_idx] += tensor_data[tensor_idx + 3] * x[neighbor_global];
                            }
                            
                            // i,j-1,k
                            if (j-1 >= 0) {
                                int neighbor_local = i + (j-1) * grid.imt + k * grid.imt * grid.jmt;
                                int neighbor_global = global_offset + neighbor_local;
                                result[global_idx] += tensor_data[tensor_idx + 4] * x[neighbor_global];
                            }
                            
                            // i,j,k+1
                            if (k+1 < grid.kmt) {
                                int neighbor_local = i + j * grid.imt + (k+1) * grid.imt * grid.jmt;
                                int neighbor_global = global_offset + neighbor_local;
                                result[global_idx] += tensor_data[tensor_idx + 5] * x[neighbor_global];
                            }
                            
                            // i,j,k-1
                            if (k-1 >= 0) {
                                int neighbor_local = i + j * grid.imt + (k-1) * grid.imt * grid.jmt;
                                int neighbor_global = global_offset + neighbor_local;
                                result[global_idx] += tensor_data[tensor_idx + 6] * x[neighbor_global];
                            }
                            
                            // The twelve diagonal neighbors - these have a weight of 0 in our current implementation
                            // but we include the structure for future extensions
                            for (int diag = 7; diag < 19; diag++) {
                                // Skip zeros to improve performance
                                if (tensor_data[tensor_idx + diag] != 0.0f) {
                                    // Determine the neighbor based on diagonal index
                                    int di = 0, dj = 0, dk = 0;
                                    
                                    // This mapping depends on how the 19-point stencil is defined
                                    // The following is just an example mapping for the diagonal elements
                                    switch (diag) {
                                        case 7:  di=1;  dj=1;  dk=0;  break; // i+1,j+1,k
                                        case 8:  di=1;  dj=-1; dk=0;  break; // i+1,j-1,k
                                        case 9:  di=-1; dj=1;  dk=0;  break; // i-1,j+1,k
                                        case 10: di=-1; dj=-1; dk=0;  break; // i-1,j-1,k
                                        case 11: di=1;  dj=0;  dk=1;  break; // i+1,j,k+1
                                        case 12: di=1;  dj=0;  dk=-1; break; // i+1,j,k-1
                                        case 13: di=-1; dj=0;  dk=1;  break; // i-1,j,k+1
                                        case 14: di=-1; dj=0;  dk=-1; break; // i-1,j,k-1
                                        case 15: di=0;  dj=1;  dk=1;  break; // i,j+1,k+1
                                        case 16: di=0;  dj=1;  dk=-1; break; // i,j+1,k-1
                                        case 17: di=0;  dj=-1; dk=1;  break; // i,j-1,k+1
                                        case 18: di=0;  dj=-1; dk=-1; break; // i,j-1,k-1
                                    }
                                    
                                    // Check if the neighbor is within bounds
                                    int ni = i + di;
                                    int nj = j + dj;
                                    int nk = k + dk;
                                    
                                    if (ni >= 0 && ni < grid.imt && 
                                        nj >= 0 && nj < grid.jmt && 
                                        nk >= 0 && nk < grid.kmt) {
                                        int neighbor_local = ni + nj * grid.imt + nk * grid.imt * grid.jmt;
                                        int neighbor_global = global_offset + neighbor_local;
                                        result[global_idx] += tensor_data[tensor_idx + diag] * x[neighbor_global];
                                    }
                                }
                            }
                        }
                    }
                    else if (method == DiscretizationMethod::SEM || method == DiscretizationMethod::SPECTRAL) {
                        // For spectral methods, the matrix would need a different implementation
                        // of neighbor relationships, which is not fully implemented yet
                        DEBUG_DETAIL("Spectral/SEM matrix-vector multiplication not fully implemented");
                    }
                }
            }
        }
    }
    
    // Handle interfaces between cavities
    // Loop through all interfaces
    for (const auto& interface : interfaces) {
        int src_cavity_id = interface.src_cavity_id;
        int dst_cavity_id = interface.dst_cavity_id;
        
        const Grid3D& src_grid = cavity_grids[src_cavity_id];
        const Grid3D& dst_grid = cavity_grids[dst_cavity_id];
        
        int src_offset = cavity_offsets[src_cavity_id];
        int dst_offset = cavity_offsets[dst_cavity_id];
        
        // Process each point in the interface region
        for (int k = interface.src_k_start; k <= interface.src_k_end; k++) {
            for (int j = interface.src_j_start; j <= interface.src_j_end; j++) {
                for (int i = interface.src_i_start; i <= interface.src_i_end; i++) {
                    // Source point
                    int src_local = i + j * src_grid.imt + k * src_grid.imt * src_grid.jmt;
                    int src_global = src_offset + src_local;
                    
                    // Destination point
                    int dst_i = interface.dst_i_start + (i - interface.src_i_start);
                    int dst_j = interface.dst_j_start + (j - interface.src_j_start);
                    int dst_k = interface.dst_k_start + (k - interface.src_k_start);
                    
                    int dst_local = dst_i + dst_j * dst_grid.imt + dst_k * dst_grid.imt * dst_grid.jmt;
                    int dst_global = dst_offset + dst_local;
                    
                    // Update using the interface coupling
                    // This is a simple approach where interface points have a coupling factor of 0.5
                    // In a more sophisticated implementation, these would be determined by the physics
                    result[src_global] += 0.5 * x[dst_global];
                    result[dst_global] += 0.5 * x[src_global];
                }
            }
        }
    }
    
    DEBUG_INFO("Matrix-vector multiplication completed");
}

// Helper function to get column index for a given stencil index
int MultiCavityMatrixBuilder::get_column_index(int global_idx, int stencil_idx) const {
    int cavity_id, local_idx;
    global_to_local(global_idx, cavity_id, local_idx);
    
    if (cavity_id < 0 || cavity_id >= static_cast<int>(cavity_grids.size())) {
        return -1;  // Invalid cavity ID
    }
    
    const Grid3D& grid = cavity_grids[cavity_id];
    int k = local_idx / (grid.imt * grid.jmt);
    int j = (local_idx - k * grid.imt * grid.jmt) / grid.imt;
    int i = local_idx - k * grid.imt * grid.jmt - j * grid.imt;
    
    if (method == DiscretizationMethod::FDM) {
        return get_stencil_column(global_idx, stencil_idx, cavity_id, i, j, k, grid);
    } else {
        return get_spectral_column(global_idx, stencil_idx, cavity_id, i, j, k, grid);
    }
}

// Helper function to get column index for FDM stencil
int MultiCavityMatrixBuilder::get_stencil_column(int row, int stencil_idx, int cavity_id, 
                                               int i, int j, int k, const Grid3D& grid) const {
    // Map stencil index to neighbor offset
    int ni = i, nj = j, nk = k;
    
    switch (stencil_idx) {
        case 0:  return row; // Diagonal element
        case 1:  ni = i + 1; break; // i+1,j,k
        case 2:  ni = i - 1; break; // i-1,j,k
        case 3:  nj = j + 1; break; // i,j+1,k
        case 4:  nj = j - 1; break; // i,j-1,k
        case 5:  nk = k + 1; break; // i,j,k+1
        case 6:  nk = k - 1; break; // i,j,k-1
        case 7:  ni = i + 1; nj = j + 1; break; // i+1,j+1,k
        case 8:  ni = i + 1; nj = j - 1; break; // i+1,j-1,k
        case 9:  ni = i - 1; nj = j + 1; break; // i-1,j+1,k
        case 10: ni = i - 1; nj = j - 1; break; // i-1,j-1,k
        case 11: ni = i + 1; nk = k + 1; break; // i+1,j,k+1
        case 12: ni = i + 1; nk = k - 1; break; // i+1,j,k-1
        case 13: ni = i - 1; nk = k + 1; break; // i-1,j,k+1
        case 14: ni = i - 1; nk = k - 1; break; // i-1,j,k-1
        case 15: nj = j + 1; nk = k + 1; break; // i,j+1,k+1
        case 16: nj = j + 1; nk = k - 1; break; // i,j+1,k-1
        case 17: nj = j - 1; nk = k + 1; break; // i,j-1,k+1
        case 18: nj = j - 1; nk = k - 1; break; // i,j-1,k-1
        default: return -1; // Invalid stencil index
    }
    
    // Check if neighbor is within this cavity
    if (ni >= 0 && ni < grid.imt && 
        nj >= 0 && nj < grid.jmt && 
        nk >= 0 && nk < grid.kmt) {
        // Neighbor is in the same cavity
        int neighbor_local = ni + nj * grid.imt + nk * grid.imt * grid.jmt;
        return local_to_global(cavity_id, neighbor_local);
    }
    
    // Check if this is an interface point
    for (const auto& interface : interfaces) {
        // Source interface
        if (interface.src_cavity_id == cavity_id &&
            i >= interface.src_i_start && i <= interface.src_i_end &&
            j >= interface.src_j_start && j <= interface.src_j_end &&
            k >= interface.src_k_start && k <= interface.src_k_end) {
            
            // Calculate corresponding destination point
            int di = interface.dst_i_start + (i - interface.src_i_start);
            int dj = interface.dst_j_start + (j - interface.src_j_start);
            int dk = interface.dst_k_start + (k - interface.src_k_start);
            
            return local_to_global_3d(interface.dst_cavity_id, di, dj, dk);
        }
        
        // Destination interface
        if (interface.dst_cavity_id == cavity_id) {
            // Calculate relative position in destination coordinates
            int rel_i = i - interface.dst_i_start;
            int rel_j = j - interface.dst_j_start;
            int rel_k = k - interface.dst_k_start;
            
            // Check if this point is in the destination region
            if (rel_i >= 0 && rel_i <= (interface.src_i_end - interface.src_i_start) &&
                rel_j >= 0 && rel_j <= (interface.src_j_end - interface.src_j_start) &&
                rel_k >= 0 && rel_k <= (interface.src_k_end - interface.src_k_start)) {
                
                // Calculate corresponding source point
                int si = interface.src_i_start + rel_i;
                int sj = interface.src_j_start + rel_j;
                int sk = interface.src_k_start + rel_k;
                
                return local_to_global_3d(interface.src_cavity_id, si, sj, sk);
            }
        }
    }
    
    return -1; // Not a valid neighbor
}

// Helper function to get column index for SEM/Spectral stencil
int MultiCavityMatrixBuilder::get_spectral_column(int row, int stencil_idx, int cavity_id,
                                                int i, int j, int k, const Grid3D& grid) const {
    // For SEM/Spectral methods, we need to determine which dimension this stencil point belongs to
    
    // For safety, ensure we don't exceed array bounds
    if (stencil_idx >= num_diagonals) {
        return -1;
    }
    
    // For SEM/Spectral methods, connections depend on the specific discretization
    // In a fully implemented spectral method, connections would be determined by
    // the spectral basis functions and quadrature points
    
    // The diagonal element (self-connection)
    if (stencil_idx == 0) {
        return row;
    }
    
    // For a true spectral method implementation, we would return connections based on
    // spectral basis functions. For now, we'll use a simplified approach:
    
    // Determine which dimension and direction this stencil point corresponds to
    int dim = (stencil_idx - 1) / 2;  // 0=x, 1=y, 2=z
    int dir = (stencil_idx - 1) % 2;  // 0=positive, 1=negative
    
    // Only process the first 7 stencil indices (diagonal + 6 neighbors)
    // This corresponds to a 7-point stencil similar to FDM but with spectral weights
    if (stencil_idx >= 1 && stencil_idx <= 6) {
        int ni = i, nj = j, nk = k;
        
        // Adjust neighbor indices based on dimension and direction
        if (dim == 0) {         // X dimension
            ni = i + (dir == 0 ? 1 : -1);
        } else if (dim == 1) {  // Y dimension
            nj = j + (dir == 0 ? 1 : -1);
        } else if (dim == 2) {  // Z dimension
            nk = k + (dir == 0 ? 1 : -1);
        }
        
        // Check if neighbor is within this cavity
        if (ni >= 0 && ni < grid.imt && 
            nj >= 0 && nj < grid.jmt && 
            nk >= 0 && nk < grid.kmt) {
            // Neighbor is in the same cavity
            int neighbor_local = ni + nj * grid.imt + nk * grid.imt * grid.jmt;
            return local_to_global(cavity_id, neighbor_local);
        }
        
        // Check if this is an interface point
        for (const auto& interface : interfaces) {
            // Source interface
            if (interface.src_cavity_id == cavity_id &&
                i >= interface.src_i_start && i <= interface.src_i_end &&
                j >= interface.src_j_start && j <= interface.src_j_end &&
                k >= interface.src_k_start && k <= interface.src_k_end) {
                
                // Calculate corresponding destination point
                int di = interface.dst_i_start + (i - interface.src_i_start);
                int dj = interface.dst_j_start + (j - interface.src_j_start);
                int dk = interface.dst_k_start + (k - interface.src_k_start);
                
                // Adjust for the neighbor direction
                if (dim == 0) {         // X dimension
                    di += (dir == 0 ? 1 : -1);
                } else if (dim == 1) {  // Y dimension
                    dj += (dir == 0 ? 1 : -1);
                } else if (dim == 2) {  // Z dimension
                    dk += (dir == 0 ? 1 : -1);
                }
                
                // Check if the destination point is valid
                const Grid3D& dst_grid = cavity_grids[interface.dst_cavity_id];
                if (di >= 0 && di < dst_grid.imt && 
                    dj >= 0 && dj < dst_grid.jmt && 
                    dk >= 0 && dk < dst_grid.kmt) {
                    return local_to_global_3d(interface.dst_cavity_id, di, dj, dk);
                }
            }
            
            // Destination interface
            if (interface.dst_cavity_id == cavity_id) {
                // Calculate relative position in destination coordinates
                int rel_i = i - interface.dst_i_start;
                int rel_j = j - interface.dst_j_start;
                int rel_k = k - interface.dst_k_start;
                
                // Check if this point is in the destination region
                if (rel_i >= 0 && rel_i <= (interface.src_i_end - interface.src_i_start) &&
                    rel_j >= 0 && rel_j <= (interface.src_j_end - interface.src_j_start) &&
                    rel_k >= 0 && rel_k <= (interface.src_k_end - interface.src_k_start)) {
                    
                    // Calculate corresponding source point
                    int si = interface.src_i_start + rel_i;
                    int sj = interface.src_j_start + rel_j;
                    int sk = interface.src_k_start + rel_k;
                    
                    // Adjust for the neighbor direction
                    if (dim == 0) {         // X dimension
                        si += (dir == 0 ? 1 : -1);
                    } else if (dim == 1) {  // Y dimension
                        sj += (dir == 0 ? 1 : -1);
                    } else if (dim == 2) {  // Z dimension
                        sk += (dir == 0 ? 1 : -1);
                    }
                    
                    // Check if the source point is valid
                    const Grid3D& src_grid = cavity_grids[interface.src_cavity_id];
                    if (si >= 0 && si < src_grid.imt && 
                        sj >= 0 && sj < src_grid.jmt && 
                        sk >= 0 && sk < src_grid.kmt) {
                        return local_to_global_3d(interface.src_cavity_id, si, sj, sk);
                    }
                }
            }
        }
    }
    
    // For all other stencil indices, return -1 to indicate no valid column
    return -1;
}

void MultiCavityMatrixBuilder::to_csr(const float* tensor_data, CsrMatrix& csr_matrix) const {
    std::cout << "\nConverting to CSR format..." << std::endl;
    auto start_time = std::chrono::high_resolution_clock::now();
    
    // Set up basic structure of matrix
    int total_points = 0;
    
    // For SEM/Spectral methods, there might be a mismatch between the grid dimensions
    // and the actual number of points used in the tensor data
    if (method == DiscretizationMethod::SEM || method == DiscretizationMethod::SPECTRAL) {
        // For SEM/Spectral, we need to use the total_grid_points value that was used
        // when building the coefficient matrix
        total_points = total_grid_points;
        std::cout << "DEBUG: Using total_grid_points for SEM/Spectral: " << total_points << std::endl;
    } else {
        // For FDM, we can calculate from the grid dimensions
        for (const auto& grid : cavity_grids) {
            int grid_points = grid.imt * grid.jmt * grid.kmt;
            total_points += grid_points;
            std::cout << "DEBUG: Adding grid points: " << grid.imt << "x" << grid.jmt << "x" << grid.kmt 
                      << " = " << grid_points << std::endl;
        }
    }
    
    std::cout << "DEBUG: Total points calculated: " << total_points << std::endl;
    
    // Verify total_points is reasonable
    if (total_points <= 0 || total_points > 1000000) {  // Arbitrary upper limit for sanity check
        std::cerr << "ERROR: Invalid total_points value: " << total_points << std::endl;
        csr_matrix.rows = 0;
        csr_matrix.data_size = 0;
        csr_matrix.row_off = nullptr;
        csr_matrix.cols = nullptr;
        csr_matrix.data = nullptr;
        return;
    }
    
    csr_matrix.rows = total_points;
    
    // Handle method-specific CSR conversion
    if (method == DiscretizationMethod::FDM) {
        // Standard CSR conversion for FDM method
        try {
            // First pass: count non-zeros
            int nnz = 0;
            for (int row = 0; row < total_points; row++) {
                for (int stencil_idx = 0; stencil_idx < num_diagonals; stencil_idx++) {
                    int col = get_column_index(row, stencil_idx);
                    if (col >= 0 && col < total_points) {
                        nnz++;
                    }
                }
            }

            std::cout << "DEBUG: Allocating FDM CSR arrays for " << nnz << " non-zeros" << std::endl;
            
            // Allocate CSR arrays
            csr_matrix.row_off = new int[total_points + 1];
            csr_matrix.cols = new int[nnz];
            csr_matrix.data = new double[nnz];
            csr_matrix.data_size = nnz;

            // Second pass: fill CSR arrays
            csr_matrix.row_off[0] = 0;
            int idx = 0;
            for (int row = 0; row < total_points; row++) {
                for (int stencil_idx = 0; stencil_idx < num_diagonals; stencil_idx++) {
                    int col = get_column_index(row, stencil_idx);
                    if (col >= 0 && col < total_points) {
                        float value = get_stencil_value(tensor_data, row, stencil_idx);
                        // Ensure diagonal entries are non-zero for numerical stability
                        if (row == col && std::abs(value) < 1e-10) {
                            value = 1e-10;
                        }
                        csr_matrix.cols[idx] = col;
                        csr_matrix.data[idx] = value;
                        idx++;
                    }
                }
                csr_matrix.row_off[row + 1] = idx;
            }
        } catch (const std::bad_alloc& e) {
            std::cerr << "Memory allocation failed in FDM CSR creation: " << e.what() << std::endl;
            
            // Cleanup any partial allocations
            if (csr_matrix.row_off) {
                delete[] csr_matrix.row_off;
                csr_matrix.row_off = nullptr;
            }
            if (csr_matrix.cols) {
                delete[] csr_matrix.cols;
                csr_matrix.cols = nullptr;
            }
            if (csr_matrix.data) {
                delete[] csr_matrix.data;
                csr_matrix.data = nullptr;
            }
            
            // Return an empty matrix
            csr_matrix.rows = 0;
            csr_matrix.data_size = 0;
        } catch (...) {
            std::cerr << "Unexpected error in FDM CSR creation" << std::endl;
            
            // Cleanup any partial allocations
            if (csr_matrix.row_off) {
                delete[] csr_matrix.row_off;
                csr_matrix.row_off = nullptr;
            }
            if (csr_matrix.cols) {
                delete[] csr_matrix.cols;
                csr_matrix.cols = nullptr;
            }
            if (csr_matrix.data) {
                delete[] csr_matrix.data;
                csr_matrix.data = nullptr;
            }
            
            // Return an empty matrix
            csr_matrix.rows = 0;
            csr_matrix.data_size = 0;
        }
    } else if (method == DiscretizationMethod::SEM || method == DiscretizationMethod::SPECTRAL) {
        // SEM/Spectral methods have different coefficient distribution
        // For a true spectral implementation, we should use the spectral basis functions
        try {
            // Debug output
            std::cout << "DEBUG: Creating method-specific CSR matrix for " 
                      << (method == DiscretizationMethod::SEM ? "SEM" : "Spectral")
                      << " method" << std::endl;
            
            // For SEM/Spectral methods, we'll use the first 7 stencil indices 
            // (diagonal + 6 neighbors) to create a 7-point stencil
            // This is a simplified approach compared to a full spectral implementation
            
            // Check for valid total_points to avoid malloc issues
            if (total_points <= 0) {
                std::cerr << "ERROR: Invalid total_points value: " << total_points << std::endl;
                csr_matrix.rows = 0;
                csr_matrix.data_size = 0;
                csr_matrix.row_off = nullptr;
                csr_matrix.cols = nullptr;
                csr_matrix.data = nullptr;
                return;
            }
            
            // First pass: count non-zeros
            // For SEM/Spectral, we'll use a 7-point stencil (diagonal + 6 neighbors)
            int max_stencil_idx = std::min(7, num_diagonals);
            int nnz = 0;
            for (int row = 0; row < total_points; row++) {
                for (int stencil_idx = 0; stencil_idx < max_stencil_idx; stencil_idx++) {
                    int col = get_column_index(row, stencil_idx);
                    if (col >= 0 && col < total_points) {
                        nnz++;
                    }
                }
            }
            
            std::cout << "DEBUG: Creating CSR matrix with " << total_points 
                      << " rows and " << nnz << " non-zeros" << std::endl;
            
            // Allocate memory for CSR arrays with careful error handling
            try {
                // Allocate arrays one by one with size checks
                std::cout << "DEBUG: Allocating row_off array of size " << (total_points + 1) << std::endl;
                if (total_points + 1 <= 0) {
                    throw std::bad_alloc();
                }
                csr_matrix.row_off = new int[total_points + 1];
                
                std::cout << "DEBUG: Allocating cols array of size " << nnz << std::endl;
                if (nnz <= 0) {
                    throw std::bad_alloc();
                }
                csr_matrix.cols = new int[nnz];
                
                std::cout << "DEBUG: Allocating data array of size " << nnz << std::endl;
                if (nnz <= 0) {
                    throw std::bad_alloc();
                }
                csr_matrix.data = new double[nnz];
                csr_matrix.data_size = nnz;
                
                // Second pass: fill CSR arrays
                csr_matrix.row_off[0] = 0;
                int idx = 0;
                for (int row = 0; row < total_points; row++) {
                    for (int stencil_idx = 0; stencil_idx < max_stencil_idx; stencil_idx++) {
                        int col = get_column_index(row, stencil_idx);
                        if (col >= 0 && col < total_points) {
                            float value = get_stencil_value(tensor_data, row, stencil_idx);
                            
                            // Check if this is a point in Cavity 1
                            int cavity_id, local_idx;
                            global_to_local(row, cavity_id, local_idx);
                            
                            if (cavity_id == 1) {
                                const Grid3D& grid = cavity_grids[cavity_id];
                                int k = local_idx / (grid.imt * grid.jmt);
                                int j = (local_idx - k * grid.imt * grid.jmt) / grid.imt;
                                int i = local_idx - k * grid.imt * grid.jmt - j * grid.imt;
                                
                                bool is_boundary = (i == 0 || i == grid.imt-1 || 
                                                   j == 0 || j == grid.jmt-1 || 
                                                   k == 0 || k == grid.kmt-1);
                                
                                if (is_boundary) {
                                    // For boundary points in Cavity 1, only include the diagonal term
                                    if (row != col) {
                                        continue; // Skip off-diagonal terms for boundary points
                                    }
                                    value = 1.0; // Set diagonal to 1.0 for boundary points
                                } else {
                                    // For interior points in Cavity 1, apply special handling
                                    if (row == col) {
                                        // Keep the diagonal term as is
                                    } else if (stencil_idx >= 1 && stencil_idx <= 6) {
                                        // Direct neighbors - apply scaling factor
                                        value /= 2.78;
                                    } else {
                                        // Skip diagonal neighbors for interior points in Cavity 1
                                        continue;
                                    }
                                }
                            }
                            
                            // Ensure diagonal entries are non-zero for numerical stability
                            if (row == col && std::abs(value) < 1e-10) {
                                value = 1e-10;
                            }
                            
                            csr_matrix.cols[idx] = col;
                            csr_matrix.data[idx] = value;
                            idx++;
                        }
                    }
                    csr_matrix.row_off[row + 1] = idx;
                }
                
                std::cout << "DEBUG: Method-specific CSR matrix created successfully" << std::endl;
                
            } catch (const std::bad_alloc& e) {
                std::cerr << "Memory allocation failed in SEM/Spectral CSR creation: " << e.what() << std::endl;
                
                // Cleanup any partial allocations
                if (csr_matrix.row_off) {
                    delete[] csr_matrix.row_off;
                    csr_matrix.row_off = nullptr;
                }
                if (csr_matrix.cols) {
                    delete[] csr_matrix.cols;
                    csr_matrix.cols = nullptr;
                }
                if (csr_matrix.data) {
                    delete[] csr_matrix.data;
                    csr_matrix.data = nullptr;
                }
                
                // Return an empty matrix
                csr_matrix.rows = 0;
                csr_matrix.data_size = 0;
            } catch (...) {
                std::cerr << "Unexpected error in SEM/Spectral CSR creation" << std::endl;
                
                // Cleanup any partial allocations
                if (csr_matrix.row_off) {
                    delete[] csr_matrix.row_off;
                    csr_matrix.row_off = nullptr;
                }
                if (csr_matrix.cols) {
                    delete[] csr_matrix.cols;
                    csr_matrix.cols = nullptr;
                }
                if (csr_matrix.data) {
                    delete[] csr_matrix.data;
                    csr_matrix.data = nullptr;
                }
                
                // Return an empty matrix
                csr_matrix.rows = 0;
                csr_matrix.data_size = 0;
            }
        } catch (...) {
            std::cerr << "Unexpected error in SEM/Spectral CSR creation" << std::endl;
            
            // Return an empty matrix
            csr_matrix.rows = 0;
            csr_matrix.data_size = 0;
            csr_matrix.row_off = nullptr;
            csr_matrix.cols = nullptr;
            csr_matrix.data = nullptr;
        }
    }

    auto end_time = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
    std::cout << "Conversion to CSR completed in " << duration.count() << "ms" << std::endl;
    std::cout << "CSR matrix: " << csr_matrix.rows << " rows, " << csr_matrix.data_size << " non-zeros" << std::endl;
}

// Helper function to get stencil value from tensor data
float MultiCavityMatrixBuilder::get_stencil_value(const float* tensor_data, int global_idx, int stencil_idx) const {
    int cavity_id, local_idx;
    global_to_local(global_idx, cavity_id, local_idx);
    
    if (cavity_id < 0 || cavity_id >= static_cast<int>(cavity_grids.size())) {
        return 0.0f;
    }
    
    const Grid3D& grid = cavity_grids[cavity_id];
    int tensor_offset = cavity_id * num_diagonals * grid.total_points();
    int tensor_idx = tensor_offset + local_idx * num_diagonals + stencil_idx;
    
    return tensor_data[tensor_idx];
}

} // namespace MultiCavityMatrix 